/*
 * Copyright:  (C) 2014 EAST
 *             All rights reserved.
 *  Filename:  canbus.c
 *   Version:  1.0.0(07/30/2014)
 *    Author:  fulinux <fulinux@sina.com>
 * ChangeLog:  1, Release initial version on "07/30/2014 01:52:51 PM"
 */                 
 

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <signal.h>
#include <pthread.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <linux/can/error.h>

#include "canbus-private.h"
#include "canbus.h"

/*
 * 打印接收帧内容函数
 */
int print_rcv_frame(can_eframe_t *can)
{
    int i;
    printf("ID=%X DLC=%d ", can->can_private->frame.can_id, 
            can->can_private->frame.can_dlc);
    printf ("DATA: ");
    for (i = 0; i < 8; i++){
        printf ("[%03d]", can->can_private->frame.data[i]);
    }
    printf ("\n");

    return 0;
}

/*
 * 发送控制或数据帧
 */
int sndcanframe (struct _can_private *canp) 
{
    int ret;
    struct _can_private *pcanp = canp;

    if ((ret = write(pcanp->s, (struct can_frame *)&pcanp->frame, sizeof(struct can_frame))) 
            != sizeof(struct can_frame)){
        if(pcanp->debug)
            perror("WRITE");
        errno = EIO;
        return -1;
    }

    if(pcanp->debug == TRUE){
        int i;
        printf("ID=%X DLC=%d ", pcanp->frame.can_id,
                pcanp->frame.can_dlc);
        printf ("DATA: ");
        for (i = 0; i < 8; i++){
            printf ("<%03d>", pcanp->frame.data[i]);
        }
        printf ("\n");
    }

    return ret;
} /* ----- End of sndcanframe()  ----- */


/*
 *  Connection Mode Request to send.
 */
int sndtpcmrts (can_eframe_t *can)
{
    int i;
    uint32_t canid;
    can_eframe_t *pcan = can;

    canid = (uint32_t)(pcan->can_private->pduhead.sa) +
        (uint32_t)(pcan->can_private->pduhead.ps << 8) + 
        (uint32_t)(pcan->can_private->pduhead.dpedpp << 24); 

    if(pcan->len < 9){
        canid |= (CAN_EFF_FLAG | (uint32_t)(pcan->pgn << 8));
        pcan->can_private->frame.can_id = canid; 
        pcan->can_private->pduhead.pf = (uint8_t)(pcan->pgn >> 8) & 0xFF;
        pcan->can_private->frame.can_dlc = pcan->len;
        for(i = 0; i < 8; i++)
            pcan->can_private->frame.data[i] = pcan->msg[i];
        for(i = pcan->can_private->frame.can_dlc; i < 8; i++)
            pcan->can_private->frame.data[i] = 0xFF;
    }else{
        canid |= (CAN_EFF_FLAG | (TP_CM << 8));
        pcan->can_private->pduhead.pf = (uint8_t)(TP_CM >> 8) & 0xFF;
        pcan->can_private->frame.can_id = canid; 
        pcan->can_private->frame.can_dlc = 8;
        pcan->can_private->frame.data[0] = TP_CM_RTS;
        pcan->can_private->frame.data[1] = (uint8_t)(pcan->len % 256);
        pcan->can_private->frame.data[2] = (uint8_t)(pcan->len / 256);
        pcan->can_private->frame.data[3] = pcan->can_private->tpackets;
        pcan->can_private->frame.data[4] = pcan->can_private->mpackets;
        pcan->can_private->frame.data[5] = (uint8_t)(pcan->pgn & 0xFF);
        pcan->can_private->frame.data[6] = (uint8_t)((pcan->pgn >> 8) & 0xFF);
        pcan->can_private->frame.data[7] = (uint8_t)((pcan->pgn >> 16) & 0xFF);
    }

    return sndcanframe(pcan->can_private);

} /* ----- End of sndtpcmrts()  ----- */

/*
 * Connection Mode Clear to send.
 */
int sndtpcmcts (can_eframe_t *can) 
{
    uint32_t canid;
    can_eframe_t *pcan = can;

    canid = (uint32_t)(pcan->can_private->pduhead.sa) +
        (uint32_t)(pcan->can_private->pduhead.ps << 8) + 
        (uint32_t)(pcan->can_private->pduhead.dpedpp << 24); 
    canid |= (CAN_EFF_FLAG + (TP_CM << 8));
    pcan->can_private->pduhead.pf = (uint8_t)(TP_CM >> 8) & 0xFF;

    pcan->can_private->frame.can_id = canid; 
    pcan->can_private->frame.can_dlc = 8;
    pcan->can_private->frame.data[0] = TP_CM_CTS;
    pcan->can_private->frame.data[1] = pcan->can_private->mpackets;
    pcan->can_private->frame.data[2] = pcan->can_private->npacket;
    pcan->can_private->frame.data[3] = 0xFF;
    pcan->can_private->frame.data[4] = 0xFF;
    pcan->can_private->frame.data[5] = (uint8_t)(pcan->pgn & 0xFF);
    pcan->can_private->frame.data[6] = (uint8_t)((pcan->pgn >> 8) & 0xFF);
    pcan->can_private->frame.data[7] = (uint8_t)((pcan->pgn >> 16) & 0xFF);

    return sndcanframe(pcan->can_private);
} /* ----- End of sndtpcmcts()  ----- */

/*
 * End of Message Acknowledgment.
 */
int sndtpcmendofmsgack (can_eframe_t *can)
{
    uint32_t canid;
    can_eframe_t *pcan = can;

    canid = (uint32_t)(pcan->can_private->pduhead.sa) +
        (uint32_t)(pcan->can_private->pduhead.ps << 8) + 
        (uint32_t)(pcan->can_private->pduhead.dpedpp << 24); 
    canid |= (CAN_EFF_FLAG + (TP_CM << 8));
    pcan->can_private->pduhead.pf = (uint8_t)(TP_CM >> 8) & 0xFF;

    pcan->can_private->frame.can_id = canid; 
    pcan->can_private->frame.can_dlc = 8;
    pcan->can_private->frame.data[0] = TP_CM_EndofMsgACK;
    pcan->can_private->frame.data[1] = (uint8_t)(pcan->len % 256);
    pcan->can_private->frame.data[2] = (uint8_t)(pcan->len / 256);
    pcan->can_private->frame.data[3] = pcan->can_private->tpackets;
    pcan->can_private->frame.data[4] = 0xFF;
    pcan->can_private->frame.data[5] = (uint8_t)(pcan->pgn & 0xFF);
    pcan->can_private->frame.data[6] = (uint8_t)((pcan->pgn >> 8) & 0xFF);
    pcan->can_private->frame.data[7] = (uint8_t)((pcan->pgn >> 16) & 0xFF);

    return sndcanframe(pcan->can_private);
} /* ----- End of sndtpcmendofmsgack()  ----- */

/*
 * Connection Abort.
 */
int sndtpconnabort (can_eframe_t *can)
{
    uint32_t canid;
    can_eframe_t *pcan = can;

    canid = (uint32_t)(pcan->can_private->pduhead.sa) +
        (uint32_t)(pcan->can_private->pduhead.ps << 8) + 
        (uint32_t)(pcan->can_private->pduhead.dpedpp << 24); 
    canid |= (CAN_EFF_FLAG + (TP_CM << 8));
    pcan->can_private->pduhead.pf = (uint8_t)(TP_CM >> 8) & 0xFF;

    pcan->can_private->frame.can_id = canid; 
    pcan->can_private->frame.can_dlc = 8;
    pcan->can_private->frame.data[0] = TP_Conn_Abort;
    pcan->can_private->frame.data[1] = pcan->can_private->reason;
    pcan->can_private->frame.data[2] = 0xFF;
    pcan->can_private->frame.data[3] = 0xFF;
    pcan->can_private->frame.data[4] = 0xFF;
    pcan->can_private->frame.data[5] = (uint8_t)(pcan->pgn & 0xFF);
    pcan->can_private->frame.data[6] = (uint8_t)((pcan->pgn >> 8) & 0xFF);
    pcan->can_private->frame.data[7] = (uint8_t)((pcan->pgn >> 16) & 0xFF);

    return sndcanframe(pcan->can_private);
} /* ----- End of sndtpconnabort()  ----- */

/*
 * Broadcast Announce Message.
 */
int sndtpcmbam (can_eframe_t *can)
{
    uint32_t canid;
    can_eframe_t *pcan = can;

    canid = (uint32_t)(pcan->can_private->pduhead.sa) +
        (uint32_t)(pcan->can_private->pduhead.ps << 8) + 
        (uint32_t)(pcan->can_private->pduhead.dpedpp << 24); 
    canid |= (CAN_EFF_FLAG + (TP_CM << 8));
    pcan->can_private->pduhead.pf = (uint8_t)(TP_CM >> 8) & 0xFF;

    pcan->can_private->frame.can_id = canid; 
    pcan->can_private->frame.can_dlc = 8;
    pcan->can_private->frame.data[0] = TP_CM_BAM;
    pcan->can_private->frame.data[1] = (uint8_t)(pcan->len % 256);
    pcan->can_private->frame.data[2] = (uint8_t)(pcan->len / 256);
    pcan->can_private->frame.data[3] = pcan->can_private->tpackets;
    pcan->can_private->frame.data[4] = 0xFF;
    pcan->can_private->frame.data[5] = (uint8_t)(pcan->pgn & 0xFF);
    pcan->can_private->frame.data[6] = (uint8_t)((pcan->pgn >> 8) & 0xFF);
    pcan->can_private->frame.data[7] = (uint8_t)((pcan->pgn >> 16) & 0xFF);

    return sndcanframe(pcan->can_private);
} /* ----- End of sndtpcmbam()  ----- */

/*
 * Transport Protocol-Data Transfer.
 */
int sndtpdt (can_eframe_t *can)
{
    int i,j;
    uint32_t canid;
    can_eframe_t *pcan = can;

    canid = (uint32_t)(pcan->can_private->pduhead.sa) +
        (uint32_t)(pcan->can_private->pduhead.ps << 8) + 
        (uint32_t)(pcan->can_private->pduhead.dpedpp << 24); 
    canid |= (CAN_EFF_FLAG + (TP_DT << 8));
    pcan->can_private->pduhead.pf = (uint8_t)(TP_DT >> 8) & 0xFF;

    pcan->can_private->frame.can_id = canid; 
    pcan->can_private->frame.can_dlc = 8;
    pcan->can_private->frame.data[0] = pcan->can_private->npacket;

    for(j = 0,i = ((pcan->can_private->npacket - 1) * 7); j < 7; i++, j++){
        pcan->can_private->frame.data[j + 1] = pcan->msg[i];
    }

    return sndcanframe(pcan->can_private);
} /* ----- End of sndtpdt()  ----- */

/*
 * receive a can extend frame.
 */
int rcvcanframe (can_eframe_t *can)
{
    int ret;
    fd_set rfds;
    can_eframe_t *pcan = can;

    FD_ZERO(&rfds);
    FD_SET(pcan->can_private->s, &rfds);

    ret = select(pcan->can_private->s + 1, &rfds, NULL, NULL, pcan->can_private->timeout);
    if(0 >= ret){
        if(0 == ret)
            errno = ETIMEDOUT;
        return -1;
    }

    if (FD_ISSET(pcan->can_private->s, &rfds)){
        ret = read(pcan->can_private->s, &pcan->can_private->frame, sizeof(struct can_frame));
        if(ret != sizeof(struct can_frame)){
            errno = EBADMSG;
            return -1;
        }
    }else{
        perror("FD_ISSET");
        return -1;
    }

    return 0;
} /* ----- End of rcvcanframe()  ----- */

/*
 * Connection Mode Request to receive.
 */
int rcvtpcmrts (can_eframe_t *can)
{
    int i;
    int ret;
    uint32_t pgn;
    can_eframe_t *pcan = can;

    for(;;){
        ret  = rcvcanframe(pcan); 
        if(-1 == ret){
            return -1;
        }

        pgn = (pcan->can_private->frame.can_id >> 8) & 0x00FF00;

        if(pgn == TP_CM && pcan->can_private->frame.data[0] == TP_CM_RTS){
            pcan->len = ((uint16_t)pcan->can_private->frame.data[2] << 8) + 
                ((uint16_t)pcan->can_private->frame.data[1]);
            if(MSG_MAXSIZE < pcan->len){
                errno = ERANGE;
                return -1;
            }
            pcan->can_private->tpackets = pcan->can_private->frame.data[3];
            pcan->can_private->mpackets = pcan->can_private->frame.data[4];
            pcan->pgn = (uint32_t)pcan->can_private->frame.data[5] + 
                (uint32_t)(pcan->can_private->frame.data[6] << 8) + 
                (uint32_t)(pcan->can_private->frame.data[7] << 16);

            if(0 < pcan->can_private->pgnfilter_nr && NULL != pcan->can_private->pgnfilter){
                for(i = 0; i < pcan->can_private->pgnfilter_nr; i++){
                    if((pcan->pgn & pcan->can_private->pgnfilter[i].mask) ==
                            (pcan->can_private->pgnfilter[i].pgn & pcan->can_private->pgnfilter[i].mask)){
                        ret = 0;
                        break;
                    }
                    ret = -1;
                }

                if(ret)
                    continue;
            }
        }else if(pgn > 255 && pgn != TP_CM && pgn != TP_DT){
            pcan->len = pcan->can_private->frame.can_dlc;
            pcan->can_private->tpackets = 1;
            pcan->can_private->mpackets = 1;
            pcan->pgn = pgn;
            pcan->can_private->pduhead.pf = (uint8_t)(pgn >> 8) & 0xFF;
            for(i = 0; i < 8; i++)
                pcan->msg[i] = pcan->can_private->frame.data[i];
        }else{
            continue;
        }

        break;
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    return pcan->len;
} /* ----- End of rcvtpcmrts()  ----- */

/*
 * Connection Mode Clear to receive.
 */
int rcvtpcmcts (can_eframe_t *can)
{
    int i;
    int ret;
    uint32_t pgn;
    can_eframe_t *pcan = can;

    for(;;){
        ret  = rcvcanframe(pcan); 
        if(ret == -1){
            return -1;
        }

        pgn = (pcan->can_private->frame.can_id >> 8) & 0x00FF00;
        if(pgn != TP_CM || pcan->can_private->frame.data[0] != TP_CM_CTS){
            continue;
        }

#if 1
        pgn = (uint32_t)pcan->can_private->frame.data[5] + 
            (uint32_t)(pcan->can_private->frame.data[6] << 8) + 
            (uint32_t)(pcan->can_private->frame.data[7] << 16);

        if(0 < pcan->can_private->pgnfilter_nr && NULL != pcan->can_private->pgnfilter){
            for(i = 0; i < pcan->can_private->pgnfilter_nr; i++){
                if((pgn & pcan->can_private->pgnfilter[i].mask) ==
                        (pcan->can_private->pgnfilter[i].pgn & pcan->can_private->pgnfilter[i].mask)){
                    pcan->pgn = pgn;
                    ret = 0;
                    break;
                }
                ret = -1;
            }

            if(ret)
                continue;
            else
                break;
        }
#endif
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    return ret;
} /* ----- End of rcvtpcmcts()  ----- */

/*
 * End of Message Acknowledgment.
 */
int rcvtpcmendofmsgack (can_eframe_t *can)
{
    int i;
    int ret;
    uint16_t msize;
    uint32_t pgn;
    can_eframe_t *pcan = can;

    for(;;){
        ret  = rcvcanframe(pcan); 
        if(ret == -1){
            return -1;
        }

        pgn = (pcan->can_private->frame.can_id >> 8) & 0x00FF00;
        if(pgn != TP_CM || pcan->can_private->frame.data[0] != TP_CM_EndofMsgACK){
            continue;
        }

        pgn = (uint32_t)pcan->can_private->frame.data[5] + 
            (uint32_t)(pcan->can_private->frame.data[6] << 8) + 
            (uint32_t)(pcan->can_private->frame.data[7] << 16);

        if(0 < pcan->can_private->pgnfilter_nr && 
                NULL != pcan->can_private->pgnfilter){
            for(i = 0; i < pcan->can_private->pgnfilter_nr; i++){
                if((pgn & pcan->can_private->pgnfilter[i].mask) ==
                        (pcan->can_private->pgnfilter[i].pgn & 
                         pcan->can_private->pgnfilter[i].mask)){
                    pcan->pgn = pgn;
                    ret = 0;
                    break;
                }
                ret = -1;
            }


            if(ret)
                continue;
            else
                break;
        }

        break;
    }

    msize = (uint16_t)((pcan->can_private->frame.data[2] << 8) + pcan->can_private->frame.data[1]);

    if(pcan->len != msize){ 
        errno = EPROTO;
        if(pcan->can_private->debug){
            perror("TOTALL MSG SIZE");
        }
        return -1;
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    return ret;
} /* ----- End of rcvtpcmendofmsgack()  ----- */

/*
 * Connection Abort.
 */
int rcvtpconnabort (can_eframe_t *can)
{
    int ret;
    uint32_t pgn;
    can_eframe_t *pcan = can;

    for(;;){
        ret  = rcvcanframe(pcan); 
        if(ret == -1){
            return -1;
        }

        pgn = (pcan->can_private->frame.can_id >> 8) & 0x00FF00;

        if(pgn != TP_CM || pcan->can_private->frame.data[0] != TP_Conn_Abort){
            continue;
        }
        break;
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    return ret;
} /* ----- End of rcvtpconnabort()  ----- */

/*
 * Broadcast Announce Message.
 */
int rcvtpcmbam (can_eframe_t *can)
{
    int ret;
    uint32_t pgn;
    can_eframe_t *pcan = can;

    for(;;){
        ret  = rcvcanframe(pcan); 
        if(ret == -1){
            return -1;
        }

        pgn = (pcan->can_private->frame.can_id >> 8) & 0x00FF00;

        if(pgn != TP_CM || pcan->can_private->frame.data[0] != TP_CM_BAM){
            continue;
        }
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    return ret;
} /* ----- End of rcvtpcmbam()  ----- */

/*
 * Transport Protocol-Data Transfer.
 */
int rcvtpdt (can_eframe_t *can)
{
    int i,j;
    int ret;
    uint32_t pgn;
    can_eframe_t *pcan = can;

    for(;;){
        ret  = rcvcanframe(pcan); 
        if(ret == -1){
            return -1;
        }

        pgn = (pcan->can_private->frame.can_id >> 8) & 0x00FF00;

        if(pgn != TP_DT || pcan->can_private->npacket != pcan->can_private->frame.data[0]){
            continue;
        }

        for(j = 0, i = ((pcan->can_private->npacket - 1) * 7); j < 7; j++, i++){
            pcan->msg[i] = pcan->can_private->frame.data[j + 1];
        }

        pcan->can_private->npacket++;

        if(pcan->can_private->debug == TRUE){
            print_rcv_frame(pcan);
        }

        break;
    }

    if(pcan->can_private->debug == TRUE){
        print_rcv_frame(pcan);
    }

    return ret;
} /* ----- End of rcvtpdt()  ----- */

/*
 * Send multiple packets.
 */
int can_send (can_eframe_t *can) 
{
    int i;
    int ret;
    int tpackets;
    struct timeval timeout;
    struct pgn_filter pfilter[1];
    can_eframe_t *pcan = can;

    if(NULL == pcan || NULL == pcan->msg || pcan->len > MSG_MAXSIZE){ 
        errno = EINVAL; 
        if(pcan->can_private->debug)
            perror("PCAN MSG MSGSIZE");
        return -1;
    }

    if(pcan->len > 8){
        pfilter[0].pgn = pcan->pgn;
        pfilter[0].mask = 0xFF00;
        can_setfilter(pcan, NULL, 0, pfilter, sizeof(pfilter));
    }else
        can_setfilter(pcan, NULL, 0, NULL, 0);

    pcan->can_private->mpackets = 255;
    pcan->can_private->reason = 0;

    tpackets = (int)(pcan->len/7) + (int)((pcan->len%7)?1:0);
    pcan->can_private->tpackets = tpackets;

    ret = sndtpcmrts(pcan);
    if(ret == -1){
        if(pcan->can_private->debug)
            perror("SEND TP.CM_RTS");
        return -1;
    }

    if(pcan->len < 9){
        printf ("\n");
        return pcan->len;
    }

    if((tpackets * 7) > MSG_MAXSIZE){
        for(i = pcan->len; i < MSG_MAXSIZE; i++){
            pcan->msg[i] = 0xFF;
        }
    }else{
        for(i = pcan->len; i < (tpackets * 7); i++){
            pcan->msg[i] = 0xFF;
        }
    }

    pcan->can_private->npacket = 1;

    pcan->can_private->timeout = &timeout;
    pcan->can_private->timeout->tv_sec = 0;
    pcan->can_private->timeout->tv_usec = TTHREE;

    for(; tpackets;){
        ret = rcvtpcmcts(pcan);
        if(ret == -1){
            if(pcan->can_private->debug)
                perror("RECV TP.CMCTS");
            errno = EPROTO;
            return -1;
        }

        if(pcan->can_private->frame.data[1] == 0){ /* Limit sending */
            pcan->can_private->timeout->tv_sec = 0;
            pcan->can_private->timeout->tv_usec = TFOUR;
            continue;
        }else{
            pcan->can_private->timeout->tv_sec = 0;
            pcan->can_private->timeout->tv_usec = TTHREE;
        }

        /* rts allow packets < cts allow packets */
        if(pcan->can_private->mpackets < pcan->can_private->frame.data[1]){ 
            if(pcan->can_private->debug)
                printf("RTS ALLOW PACKETS < CTS ALLOW PACKETS\n");
            errno = EINVAL;
            return -1;
        }else{
            pcan->can_private->mpackets = pcan->can_private->frame.data[1];
        }

        if(pcan->can_private->frame.data[1] == 0xFF){
            pcan->can_private->mpackets = pcan->can_private->tpackets;
        }

        /* it's wrong of next packet ? */
        if(pcan->can_private->npacket != pcan->can_private->frame.data[2]){
            if(pcan->can_private->debug)
                printf ("NEXT PACKET NO EQUAL\n");
            errno = EINVAL;
            return -1;
        }

        /* send the allowed number of packets. */
        for(i = 0; i < pcan->can_private->mpackets; i++){ 
            ret = sndtpdt(pcan);
            if(ret == -1){
                perror("SEND TP.DT");
                return -1;
            }
            pcan->can_private->npacket++;
        }

        tpackets -= pcan->can_private->mpackets;
        if(tpackets < 0){
            break;
        }
    }

    pcan->can_private->timeout->tv_sec = 0;
    pcan->can_private->timeout->tv_usec = TTHREE;
    ret = rcvtpcmendofmsgack(pcan);
    if(ret == -1){ 
        if(pcan->can_private->debug)
            perror("RECV TPCMENDOFMSGACK");
        return -1;
    }
    printf ("\n");

    return 0;
} /* ----- End of can_send()  ----- */


/*
 * timeout为超时时间参数.
 */
int can_recv (can_eframe_t *can, struct timeval *timeout)
{
    int ret;
    int prio;
    int tpackets;
    struct timeval timetmp;
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    pcan->pgn = 0;
    pcan->can_private->timeout = timeout;

    ret = rcvtpcmrts(pcan);
    if(ret == -1){
        if(pcan->can_private->debug)
            perror("RECV TP.CM_RTS");
        return -1;
    }

    if(pcan->len < 9){
        printf ("\n");
        return pcan->len;
    }

    tpackets = pcan->can_private->tpackets;
    if(tpackets < 0){
        perror("TPACKETS < 0");
        return -1;
    }

    pcan->can_private->npacket = 1;

    /* 
     * 因为在涉及到周期发送"发送列表"中的报文时
     * 会出现cts报文过早发送，导致发送方没有收到
     * cts报文而超时. 这里延时后基本上就没有问
     * 题了, 不过还需要更多的实际检验. 
     */

    /* usleep(3000); */

    /* 设置为接收的优先级 */
    prio = (pcan->can_private->pduhead.dpedpp >> 2) & 0x07;
    pcan->can_private->pduhead.dpedpp &= ~0x1C;
    pcan->can_private->pduhead.dpedpp |= 
        (pcan->can_private->frame.can_id >> 24) & 0x1C;

    ret = sndtpcmcts(pcan);
    if(ret == -1){
        perror("SEND TP.CM_CTS");
        return -1;
    }

    timetmp.tv_sec = pcan->can_private->timeout->tv_sec;
    timetmp.tv_usec = pcan->can_private->timeout->tv_usec;
    for(;tpackets;){
        if((timetmp.tv_sec * 1000000 + timetmp.tv_usec) < TONE){
            pcan->can_private->timeout->tv_sec = 0;
            pcan->can_private->timeout->tv_usec = timetmp.tv_usec;
        }else{
            if(timetmp.tv_usec > TONE){
                timetmp.tv_usec -= TONE;
            }else{
                timetmp.tv_sec--;
                timetmp.tv_usec += (1000000 - TONE);
            }
            pcan->can_private->timeout->tv_sec = 0;
            pcan->can_private->timeout->tv_usec = TONE;
        }
        ret = rcvtpdt(pcan);
        if(ret == -1){
            perror("RECV TP.DT");
            return -1;
        }
        tpackets--;
    }

    pcan->can_private->timeout->tv_sec = 0;
    pcan->can_private->timeout->tv_sec = TR;
    ret = sndtpcmendofmsgack(pcan);
    if(ret == -1){
        perror("SEND TP.CM_ENDMSKACK");
        return -1;
    }

    /* 恢复默认优先级 */
    pcan->can_private->pduhead.dpedpp &= ~0x1C; 
    pcan->can_private->pduhead.dpedpp |= ((prio << 2) & 0x1C); 

    printf ("\n");

    return pcan->len;
} /* ----- End of can_recv()  ----- */

/* 
 * 设置是否打印调试信息.
 */
int can_getdebug(can_eframe_t *can)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    return pcan->can_private->debug;
}

/* 
 * 设置是否打印调试信息.
 */
int can_setdebug(can_eframe_t *can, int flag)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    pcan->can_private->debug = flag;

    return 0;
}

/* 
 * 获取优先级.
 */
int can_getprio(can_eframe_t *can)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    return ((pcan->can_private->pduhead.dpedpp >> 2) & 0x07);
}

/* 
 * 设置优先级.
 */
int can_setprio(can_eframe_t *can, uint8_t prio)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    pcan->can_private->pduhead.dpedpp &= ~0x1C; 
    pcan->can_private->pduhead.dpedpp |= ((prio << 2) & 0x1C); 

    return 0;
}

/* 
 * 获取PDU格式.
 */
int can_getpduf(can_eframe_t *can)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    return (pcan->can_private->pduhead.dpedpp & 0x03);
}

/* 
 * 设置PDU格式.
 */
int can_setpduf(can_eframe_t *can, uint8_t pduf)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    pcan->can_private->pduhead.dpedpp &= ~0x03;
    pcan->can_private->pduhead.dpedpp |= (pduf & 0x03);

    return 0;
}

/* 
 * 获取目的地址.
 */
int can_getdst(can_eframe_t *can)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    return (pcan->can_private->pduhead.ps);
}

/* 
 * 设置目的地址.
 */
int can_setdst(can_eframe_t *can, uint8_t dst)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    pcan->can_private->pduhead.ps = dst;

    return 0;
}

/* 
 * 获取源地址.
 */
int can_getsrc(can_eframe_t *can)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    return (pcan->can_private->pduhead.sa);
}

/* 
 * 设置源地址.
 */
int can_setsrc(can_eframe_t *can, uint8_t src)
{
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    pcan->can_private->pduhead.sa = src;

    return 0;
}

/* 
 * 设置过滤规则, 基本功can id的过滤, 
 * 也包括pgn的过滤, 这在进行传输协议
 * 接收指定报文时是很有用效果的. 
 */
int can_setfilter(can_eframe_t *can, struct can_filter *canidfilter, 
        size_t canidlen, struct pgn_filter *pgnfilter, size_t pgnfilterlen) 
{
    /* can_id filter */
    int idfilter_len = 0;
    struct can_filter *idfilter = NULL;
    can_eframe_t *pcan = can;

    if(NULL == pcan){
        errno = EINVAL;
        return -1;
    }

    /* 添加PGN过滤的同时, 添加TP_CM和TP_DT过滤. */
    if(NULL != pgnfilter && 0 < pgnfilterlen){
        struct can_filter filter[2];

        pcan->can_private->pgnfilter_nr = pgnfilterlen / sizeof(struct pgn_filter);

        if(pcan->can_private->pgnfilter != NULL)
            free(pcan->can_private->pgnfilter);

        pcan->can_private->pgnfilter = (struct pgn_filter *)malloc(pgnfilterlen);
        memcpy(pcan->can_private->pgnfilter, pgnfilter, pgnfilterlen);

        filter[0].can_id = (CAN_EFF_FLAG | (TP_CM << 8));
        filter[0].can_mask = CAN_EFF_FLAG | 0xFF0000;
        filter[1].can_id = (CAN_EFF_FLAG | (TP_DT << 8));
        filter[1].can_mask = CAN_EFF_FLAG | 0xFF0000;
        idfilter_len = sizeof(filter);
        idfilter = (struct can_filter *)malloc(sizeof(filter));
        memcpy((void *)idfilter, (void *)filter, sizeof(filter)); 
    }

    /* 同时有can_id过滤盒PGN过滤时, 重新分配缓存空间. */
    if(NULL != canidfilter && 0 < canidlen){
        idfilter_len += canidlen;
        idfilter = (struct can_filter *)realloc(idfilter, idfilter_len);
        memcpy((void *)idfilter + (idfilter_len - canidlen), 
                (void *)canidfilter, canidlen);

        if(-1 == setsockopt(pcan->can_private->s, SOL_CAN_RAW, 
                    CAN_RAW_FILTER, (struct can_filter *)idfilter, idfilter_len)){
            return -1;
        }
    }else{
        if(-1 == setsockopt(pcan->can_private->s, SOL_CAN_RAW, 
                    CAN_RAW_FILTER, (struct can_filter *)idfilter, idfilter_len)){
            return -1;
        }
    }

    /* setsockopt设置后，就无需这个buf了, 将其释放掉. */
    if(idfilter != NULL)
        free(idfilter);

    return 0;
}

/* 
 * 求最大公约数.
 * 辗转相除法.
 */
uint64_t gcd(uint64_t m, uint64_t n)
{
    uint64_t tmp;

    if(m < n){
        tmp = m;
        m = n;
        n = tmp;
    }

    if(m % n == 0){
        return n;
    }else{
        return gcd(n, m%n);
    }
}

/* 
 * 周期发送"发送列表"中的报文,第一次调
 * 用后会自动启动一个定时器来周期地
 * 执行该函数.
 */
void *cycle2sndpkt (can_eframe_t *can)
{
    int i;
    uint64_t val = 0LL;
    uint64_t valu = 0LL;
    struct timeval period;

    pthread_setcancelstate( PTHREAD_CANCEL_ENABLE, NULL); /* 允许退出线程 */
    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); /* 设置立即取消 */

    period.tv_sec = 0;
    period.tv_usec = 0;

    val = can->can_private->psndlist[0].sndlist.itv.it_interval.tv_sec * 1000000 +
        can->can_private->psndlist[0].sndlist.itv.it_interval.tv_usec;

    for(i = 0; i < can->can_private->sndpkt_nr; i++){
        can->can_private->psndlist[i].tv.tv_sec = 0;
        can->can_private->psndlist[i].tv.tv_usec = 0;
        /* 求一个最大公约数作为定时器触发周期. */
        valu = can->can_private->psndlist[i].sndlist.itv.it_interval.tv_sec * 1000000 +
            can->can_private->psndlist[i].sndlist.itv.it_interval.tv_usec;
        val = gcd(valu, val);
    }

    period.tv_sec = val / 1000000;
    period.tv_usec = val % 1000000;

    for(;;){
        if(can->can_private->sndpkt_nr == 0 ||
                can->can_private->psndlist == NULL){
            can->can_private->pid = -1;
            can->can_private->pid = -1;
            pthread_exit((void *)0);
        }

        for(i = 0; i < can->can_private->sndpkt_nr; i++){
            /* 加锁 */
            if(pthread_mutex_trylock(&can->can_private->mutex) != 0){
                printf("MUTEX TRYLOCK FAILURE\n");
                can->can_private->pid = -1;
                pthread_exit((void *)0);
            }

            if(can->can_private->sndpkt_nr == 0 ||
                    can->can_private->psndlist == NULL){
                can->can_private->pid = -1;
                pthread_exit((void *)0);
            }

            /* 加上周期时间 */
            can->can_private->psndlist[i].tv.tv_sec += 
                (period.tv_sec * 1000000 + period.tv_usec)/1000000;
            can->can_private->psndlist[i].tv.tv_usec += 
                (period.tv_sec * 1000000 + period.tv_usec)%1000000;

            if((can->can_private->psndlist[i].tv.tv_sec * 1000000 + 
                        can->can_private->psndlist[i].tv.tv_usec) >= 
                    (can->can_private->psndlist[i].sndlist.itv.it_value.tv_sec * 1000000 + 
                     can->can_private->psndlist[i].sndlist.itv.it_value.tv_usec)){

                can->can_private->psndlist[i].tv.tv_sec = 0;
                can->can_private->psndlist[i].tv.tv_usec = 0;
                memcpy((void *)&can->can_private->psndlist[i].sndlist.itv.it_value,
                        (void *)&can->can_private->psndlist[i].sndlist.itv.it_interval,
                        sizeof(struct timeval));

                can->pgn = can->can_private->psndlist[i].sndlist.pgn;
                can_setprio(can, can->can_private->psndlist[i].sndlist.prio);
                can->len = can->can_private->psndlist[i].sndlist.len;
                memcpy(can->msg, can->can_private->psndlist[i].sndlist.msg, can->len);

                if(can_send(can) == -1){
                    perror ("CAN SEND ERROR");
                }
            }
            /* 释放锁 */
            if(pthread_mutex_unlock(&can->can_private->mutex) != 0){
                perror("MUTEX UNLOCK");
                can->can_private->pid = -1;
                pthread_exit((void *)0);
            }

        }

        /* 
         * 唤醒等待该条件变量的线程, 对应下面的函数中的
         * pthread_cond_wait.
         */
        pthread_cond_signal(&can->can_private->cond);
        usleep(val);
    }

    return NULL;
}

/*
 * 周期发送函数.
 * 实质上它会调用上面的信号函数,
 * 它是一个非阻塞的函数。
 */
int can_cycle2snd (can_eframe_t *can, struct sndlist *sndlist, size_t len)
{
    int i;
    void *tret;

    if(can == NULL){
        errno = EINVAL;
        return -1;
    }

    if(pthread_mutex_lock(&can->can_private->mutex) != 0){
        perror("MUTEX LOCK");
    }

    if(can->can_private->pid != -1){
        pthread_cancel(can->can_private->pid);
        if(pthread_join(can->can_private->pid, &tret)){
            perror("PTHREAD JOIN");
        }
        can->can_private->pid = -1;
    }

    if(can->can_private->psndlist != NULL){
        can->can_private->sndpkt_nr = 0;
        free(can->can_private->psndlist);
        can->can_private->psndlist = NULL;
    }

    if(len == 0 || sndlist == NULL){
        can->can_private->sndpkt_nr = 0;
        can->can_private->psndlist = NULL;
        if(pthread_mutex_unlock(&can->can_private->mutex) != 0){
            perror("MUTEX UNLOCK");
        }
        return 0;
    }

    can->can_private->sndpkt_nr = len / sizeof(struct sndlist);

    can->can_private->psndlist = (struct _sndlist *)malloc(can->can_private->sndpkt_nr *
            sizeof(struct _sndlist));
    for(i = 0; i < can->can_private->sndpkt_nr; i++){
        memcpy((void *)&can->can_private->psndlist[i],
                (void *)&sndlist[i], sizeof(struct sndlist));
    }

    if(pthread_create(&can->can_private->pid,NULL,
                (void *)cycle2sndpkt,(void *)can) != 0){
        perror("PTHREAD CREATE");

        if(pthread_mutex_unlock(&can->can_private->mutex) != 0){
            perror("MUTEX UNLOCK");
        }

        return -1;
    }

    /* 等待完成第一次发送 */
    pthread_cond_wait(&can->can_private->cond, &can->can_private->mutex);

    if(pthread_mutex_unlock(&can->can_private->mutex) != 0){
        perror("MUTEX UNLOCK");
    }

    return 0;
}


/* 
 * 释放初始化时申请的资源.
 */
int can_exit (can_eframe_t *can)
{
    can_eframe_t *pcan = can;

    if(-1 != pcan->can_private->s)
        close(pcan->can_private->s);

    /* 注销条件变量和互斥锁 */
    pthread_cond_destroy(&pcan->can_private->cond);
    pthread_mutex_destroy(&pcan->can_private->mutex);

    if(NULL != pcan->msg){
        free(pcan->msg);
        pcan->msg = NULL;
    }

    if(NULL != pcan->can_private->psndlist){
        free(pcan->can_private->psndlist);
        pcan->can_private->psndlist = NULL;
    }

    if(NULL != pcan->can_private){
        free(pcan->can_private);
        pcan->can_private = NULL;
    }

    if(NULL != pcan){
        free(pcan);
        pcan = NULL;
    }

    return 0;
} /* ----- End of can_exit()  ----- */

/* 
 * 参数有设备名: candev, 优先级: prio, 
 * PDU格式: pduf, 目的地址: dest, 
 * 源地址: src. 返回数据结构体指针. 
 */
can_eframe_t *can_init(char *candev, uint8_t prio, 
        uint8_t pduf, uint8_t dest, uint8_t src)
{
    struct ifreq ifr;
    struct sockaddr_can addr;
    can_eframe_t *pcan;

    if(7 < prio || 0 > prio || 3 < pduf || 0 > pduf){
        errno = EINVAL;
        return NULL;
    }

    if(strlen(candev) >= IFNAMSIZ){
        errno = EINVAL;
        return NULL;
    }

    pcan = (can_eframe_t *)malloc(sizeof(can_eframe_t));
    if(pcan == NULL){
        perror("MALLOC PCAN");
        return NULL;
    }

    pcan->pgn = 0;
    pcan->len = 0;
    pcan->msg = (uint8_t *)malloc(MSG_MAXSIZE * sizeof(uint8_t));
    if(pcan->msg == NULL){
        perror("MALLOC MSG");
        free(pcan);
        return NULL;
    }

    pcan->can_private = (struct _can_private *)malloc(sizeof(struct _can_private));
    if(pcan->can_private== NULL){
        perror("MALLOC CAN_PRIVATE");
        free(pcan->msg);
        free(pcan);
        return NULL;
    }

    pcan->can_private->pduhead.dpedpp = ((prio << 2) & 0x1C) + 
        (pduf & 0x03);
    pcan->can_private->pduhead.ps = dest;
    pcan->can_private->pduhead.sa = src;

    pcan->can_private->debug = FALSE;
    pcan->can_private->reason = 0;

    pcan->can_private->pgnfilter_nr = 0;
    pcan->can_private->pgnfilter = NULL;

    pcan->can_private->sndpkt_nr = 0;
    pcan->can_private->psndlist = NULL;

    pcan->can_private->timeout= NULL;

    pcan->can_private->pid = -1; /* pthread id init */
    /* 初始化条件变量和互斥锁 */
    if(pthread_cond_init(&pcan->can_private->cond, NULL)){
        perror("COND INIT");
        free(pcan->can_private);
        free(pcan->msg);
        free(pcan);
        return NULL;
    }

    if(pthread_mutex_init(&pcan->can_private->mutex, NULL)){
        perror("MUTEX INIT");
        pthread_cond_destroy(&pcan->can_private->cond);
        free(pcan->can_private);
        free(pcan->msg);
        free(pcan);
        return NULL;
    }

    /*  open socket */
    if ((pcan->can_private->s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("SOCKET");
        pcan->can_private->s = -1;
        can_exit(pcan);
        return NULL;
    }

    strcpy(ifr.ifr_name, candev);
    if (ioctl(pcan->can_private->s, SIOGIFINDEX, &ifr) < 0){
        perror("SIOCGIFINDEX");
        can_exit(pcan);
        return NULL;
    }

    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;

    if(bind(pcan->can_private->s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("Error in socket bind");
        can_exit(pcan);
        return NULL;
    }

#if 0
    int loopback = 0;  /* 0表示关闭, 1表示开启(默认) */
    if(setsockopt(pcan->can_private->s, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)) < 0){
        perror("Error in setsockopt");
        can_exit(pcan);
        return NULL;
    }
#endif

    return pcan;
} /* ----- End of can_init()  ----- */
